/*
 * (C) Copyright 2002 ELTEC Elektronik AG
 * Frank Gottschling <fgottschling@eltec.de>
 *
 * See file CREDITS for list of people who contributed to this
 * project.
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2 of
 * the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 * MA 02111-1307 USA
 */

/*
 * cfb_console.c
 *
 * Color Framebuffer Console driver for 8/15/16/24/32 bits per pixel.
 *
 * At the moment only the 8x16 font is tested and the font fore- and
 * background color is limited to black/white/gray colors. The Linux
 * logo can be placed in the upper left corner and additional board
 * information strings (that normaly goes to serial port) can be drawed.
 *
 * The console driver can use the standard PC keyboard interface (i8042)
 * for character input. Character output goes to a memory mapped video
 * framebuffer with little or big-endian organisation.
 * With environment setting 'console=serial' the console i/o can be
 * forced to serial port.

 The driver uses graphic specific defines/parameters/functions:

 (for SMI LynxE graphic chip)

 CONFIG_VIDEO_SMI_LYNXEM - use graphic driver for SMI 710,712,810
 VIDEO_FB_LITTLE_ENDIAN  - framebuffer organisation default: big endian
 VIDEO_HW_RECTFILL   - graphic driver supports hardware rectangle fill
 VIDEO_HW_BITBLT     - graphic driver supports hardware bit blt

 Console Parameters are set by graphic drivers global struct:

 VIDEO_VISIBLE_COLS      - x resolution
 VIDEO_VISIBLE_ROWS      - y resolution
 VIDEO_PIXEL_SIZE        - storage size in byte per pixel
 VIDEO_DATA_FORMAT       - graphical data format GDF
 VIDEO_FB_ADRS           - start of video memory

 CONFIG_I8042_KBD        - AT Keyboard driver for i8042
 VIDEO_KBD_INIT_FCT      - init function for keyboard
 VIDEO_TSTC_FCT          - keyboard_tstc function
 VIDEO_GETC_FCT          - keyboard_getc function

 CONFIG_CONSOLE_CURSOR       - on/off drawing cursor is done with delay
                   loop in VIDEO_TSTC_FCT (i8042)
 CONFIG_SYS_CONSOLE_BLINK_COUNT     - value for delay loop - blink rate
 CONFIG_CONSOLE_TIME         - display time/date in upper right corner,
                   needs CONFIG_CMD_DATE and CONFIG_CONSOLE_CURSOR
 CONFIG_VIDEO_LOGO       - display Linux Logo in upper left corner
 CONFIG_VIDEO_BMP_LOGO       - use bmp_logo instead of linux_logo
 CONFIG_CONSOLE_EXTRA_INFO   - display additional board information strings
                   that normaly goes to serial port. This define
                   requires a board specific function:
                   video_drawstring (VIDEO_INFO_X,
                         VIDEO_INFO_Y + i*VIDEO_FONT_HEIGHT,
                         info);
                   that fills a info buffer at i=row.
                   s.a: board/eltec/bab7xx.
CONFIG_VGA_AS_SINGLE_DEVICE  - If set the framebuffer device will be initialised
                   as an output only device. The Keyboard driver
                   will not be set-up. This may be used, if you
                   have none or more than one Keyboard devices
                   (USB Keyboard, AT Keyboard).

CONFIG_VIDEO_SW_CURSOR:      - Draws a cursor after the last character. No
                   blinking is provided. Uses the macros CURSOR_SET
                   and CURSOR_OFF.
CONFIG_VIDEO_HW_CURSOR:      - Uses the hardware cursor capability of the
                   graphic chip. Uses the macro CURSOR_SET.
                   ATTENTION: If booting an OS, the display driver
                   must disable the hardware register of the graphic
                   chip. Otherwise a blinking field is displayed
*/
#define  __SYLIXOS_KERNEL
#include "SylixOS.h"
#include <malloc.h>
#include <fcntl.h>
#include <sys/mman.h>
#include <string.h>
#include <stdio.h>
#include "cfb_console.h"

/*****************************************************************************/
/* Include video_fb.h after definitions of VIDEO_HW_RECTFILL etc         */
/*****************************************************************************/
#include "video_fb.h"

/*****************************************************************************/
/* Console device                                */
/*****************************************************************************/
#include <linux/types.h>
#include "video_font.h"

/*****************************************************************************/
/* Cursor definition:                                */
/* CONFIG_CONSOLE_CURSOR:  Uses a timer function (see drivers/input/i8042.c) */
/*                         to let the cursor blink. Uses the macros      */
/*                         CURSOR_OFF and CURSOR_ON.                 */
/* CONFIG_VIDEO_SW_CURSOR: Draws a cursor after the last character. No       */
/*             blinking is provided. Uses the macros CURSOR_SET  */
/*             and CURSOR_OFF.                   */
/* CONFIG_VIDEO_HW_CURSOR: Uses the hardware cursor capability of the        */
/*             graphic chip. Uses the macro CURSOR_SET.      */
/*             ATTENTION: If booting an OS, the display driver   */
/*             must disable the hardware register of the graphic */
/*             chip. Otherwise a blinking field is displayed     */
/*****************************************************************************/
#if !defined(CONFIG_CONSOLE_CURSOR) && \
    !defined(CONFIG_VIDEO_SW_CURSOR) && \
    !defined(CONFIG_VIDEO_HW_CURSOR)
/* no Cursor defined */
#define CURSOR_ON
#define CURSOR_OFF
#define CURSOR_SET
#endif

/*****************************************************************************/
/* some Macros                                   */
/*****************************************************************************/
#define VIDEO_VISIBLE_COLS  (pGD->winSizeX)
#define VIDEO_VISIBLE_ROWS  (pGD->winSizeY)
#define VIDEO_PIXEL_SIZE    (pGD->gdfBytesPP)
#define VIDEO_DATA_FORMAT   (pGD->gdfIndex)
#define VIDEO_FB_ADRS       (pGD->frameAdrs)

#ifdef  CONFIG_VIDEO_SW_CURSOR
#define CURSOR_ON
#define CURSOR_OFF
#define CURSOR_SET video_set_cursor()
#endif /* CONFIG_VIDEO_SW_CURSOR */

#define VIDEO_COLS          VIDEO_VISIBLE_COLS
#define VIDEO_ROWS          VIDEO_VISIBLE_ROWS
#define VIDEO_SIZE          (VIDEO_ROWS*VIDEO_COLS*VIDEO_PIXEL_SIZE)
#define VIDEO_PIX_BLOCKS    (VIDEO_SIZE >> 2)
#define VIDEO_LINE_LEN      (VIDEO_COLS*VIDEO_PIXEL_SIZE)
#define VIDEO_BURST_LEN     (VIDEO_COLS/8)

#define CONSOLE_ROWS        (VIDEO_ROWS / VIDEO_FONT_HEIGHT)
#define CONSOLE_COLS        (VIDEO_COLS / VIDEO_FONT_WIDTH)
#define CONSOLE_ROW_SIZE    (VIDEO_FONT_HEIGHT * VIDEO_LINE_LEN)
#define CONSOLE_ROW_FIRST   (video_console_address)
#define CONSOLE_ROW_SECOND  (video_console_address + CONSOLE_ROW_SIZE)
#define CONSOLE_ROW_LAST    (video_console_address + CONSOLE_SIZE - CONSOLE_ROW_SIZE)
#define CONSOLE_SIZE        (CONSOLE_ROW_SIZE * CONSOLE_ROWS)
#define CONSOLE_SCROLL_SIZE (CONSOLE_SIZE - CONSOLE_ROW_SIZE)

/* Macros */
#ifdef  VIDEO_FB_LITTLE_ENDIAN
#define SWAP16(x)    ((((x) & 0x00ff) << 8) | ( (x) >> 8))
#define SWAP32(x)    ((((x) & 0x000000ff) << 24) | (((x) & 0x0000ff00) << 8)|\
              (((x) & 0x00ff0000) >>  8) | (((x) & 0xff000000) >> 24) )

#if 0
#define SHORTSWAP32(x)   ((((x) & 0x000000ff) <<  8) | (((x) & 0x0000ff00) >> 8)|\
              (((x) & 0x00ff0000) <<  8) | (((x) & 0xff000000) >> 8) )
#else
#define SHORTSWAP32(x)   ( ((x) >> 16) | ((x) << 16) )
#endif

#else
#define SWAP16(x)    (x)
#define SWAP32(x)    (x)
#if defined(VIDEO_FB_16BPP_WORD_SWAP)
#define SHORTSWAP32(x)   ( ((x) >> 16) | ((x) << 16) )
#else
#define SHORTSWAP32(x)   (x)
#endif
#endif

#if defined(DEBUG) || defined(DEBUG_CFB_CONSOLE)
#define PRINTD(x)     printk(x)
#else
#define PRINTD(x)
#endif

/* Locals */
static GraphicDevice GD;
static GraphicDevice *pGD;  /* Pointer to Graphic array */

static void *video_fb_address;      /* frame buffer address */
static void *video_console_address; /* console buffer start address */

static int video_logo_height = 0;

static int console_col = 0; /* cursor col */
static int console_row = 0; /* cursor row */

static  int color_table[] = {
        BLACK_24_32BIT,
        BLUE_24_32BIT,
        GREEN_24_32BIT,
        CYAN_24_32BIT,
        RED_24_32BIT,
        MAGENTA_24_32BIT,
        YELLOW_24_32BIT,
        WHITE_24_32BIT,
        BIOS_WHITE_24_32BIT
};

static u32 eorx, fgx, bgx;  /* color pats */

static const int video_font_draw_table8[] = {
        0x00000000, 0x000000ff, 0x0000ff00, 0x0000ffff,
        0x00ff0000, 0x00ff00ff, 0x00ffff00, 0x00ffffff,
        0xff000000, 0xff0000ff, 0xff00ff00, 0xff00ffff,
        0xffff0000, 0xffff00ff, 0xffffff00, 0xffffffff };

static const int video_font_draw_table15[] = {
        0x00000000, 0x00007fff, 0x7fff0000, 0x7fff7fff };

static const int video_font_draw_table16[] = {
        0x00000000, 0x0000ffff, 0xffff0000, 0xffffffff };

static const int video_font_draw_table24[16][3] = {
        { 0x00000000, 0x00000000, 0x00000000 },
        { 0x00000000, 0x00000000, 0x00ffffff },
        { 0x00000000, 0x0000ffff, 0xff000000 },
        { 0x00000000, 0x0000ffff, 0xffffffff },
        { 0x000000ff, 0xffff0000, 0x00000000 },
        { 0x000000ff, 0xffff0000, 0x00ffffff },
        { 0x000000ff, 0xffffffff, 0xff000000 },
        { 0x000000ff, 0xffffffff, 0xffffffff },
        { 0xffffff00, 0x00000000, 0x00000000 },
        { 0xffffff00, 0x00000000, 0x00ffffff },
        { 0xffffff00, 0x0000ffff, 0xff000000 },
        { 0xffffff00, 0x0000ffff, 0xffffffff },
        { 0xffffffff, 0xffff0000, 0x00000000 },
        { 0xffffffff, 0xffff0000, 0x00ffffff },
        { 0xffffffff, 0xffffffff, 0xff000000 },
        { 0xffffffff, 0xffffffff, 0xffffffff } };

static const int video_font_draw_table32[16][4] = {
        { 0x00000000, 0x00000000, 0x00000000, 0x00000000 },
        { 0x00000000, 0x00000000, 0x00000000, 0x00ffffff },
        { 0x00000000, 0x00000000, 0x00ffffff, 0x00000000 },
        { 0x00000000, 0x00000000, 0x00ffffff, 0x00ffffff },
        { 0x00000000, 0x00ffffff, 0x00000000, 0x00000000 },
        { 0x00000000, 0x00ffffff, 0x00000000, 0x00ffffff },
        { 0x00000000, 0x00ffffff, 0x00ffffff, 0x00000000 },
        { 0x00000000, 0x00ffffff, 0x00ffffff, 0x00ffffff },
        { 0x00ffffff, 0x00000000, 0x00000000, 0x00000000 },
        { 0x00ffffff, 0x00000000, 0x00000000, 0x00ffffff },
        { 0x00ffffff, 0x00000000, 0x00ffffff, 0x00000000 },
        { 0x00ffffff, 0x00000000, 0x00ffffff, 0x00ffffff },
        { 0x00ffffff, 0x00ffffff, 0x00000000, 0x00000000 },
        { 0x00ffffff, 0x00ffffff, 0x00000000, 0x00ffffff },
        { 0x00ffffff, 0x00ffffff, 0x00ffffff, 0x00000000 },
        { 0x00ffffff, 0x00ffffff, 0x00ffffff, 0x00ffffff } };


/******************************************************************************/

static void video_drawchars (int xx, int yy, unsigned char *s, int count)
{
    u8 *cdat, *dest, *dest0;
    int rows, offset, c;

    offset = yy * VIDEO_LINE_LEN + xx * VIDEO_PIXEL_SIZE;
    dest0 = video_fb_address + offset;

    switch (VIDEO_DATA_FORMAT) {
#if 0
    case GDF__8BIT_INDEX:
    case GDF__8BIT_332RGB:
        while (count--) {
            c = *s;
            cdat = video_fontdata + c * VIDEO_FONT_HEIGHT;
            for (rows = VIDEO_FONT_HEIGHT, dest = dest0;
                 rows--;
                 dest += VIDEO_LINE_LEN) {
                u8 bits = *cdat++;

                ((u32 *) dest)[0] = (video_font_draw_table8[bits >> 4] & eorx) ^ bgx;
                ((u32 *) dest)[1] = (video_font_draw_table8[bits & 15] & eorx) ^ bgx;
            }
            dest0 += VIDEO_FONT_WIDTH * VIDEO_PIXEL_SIZE;
            s++;
        }
        break;

    case GDF_15BIT_555RGB:
        while (count--) {
            c = *s;
            cdat = video_fontdata + c * VIDEO_FONT_HEIGHT;
            for (rows = VIDEO_FONT_HEIGHT, dest = dest0;
                 rows--;
                 dest += VIDEO_LINE_LEN) {
                u8 bits = *cdat++;

                ((u32 *) dest)[0] = SHORTSWAP32 ((video_font_draw_table15 [bits >> 6] & eorx) ^ bgx);
                ((u32 *) dest)[1] = SHORTSWAP32 ((video_font_draw_table15 [bits >> 4 & 3] & eorx) ^ bgx);
                ((u32 *) dest)[2] = SHORTSWAP32 ((video_font_draw_table15 [bits >> 2 & 3] & eorx) ^ bgx);
                ((u32 *) dest)[3] = SHORTSWAP32 ((video_font_draw_table15 [bits & 3] & eorx) ^ bgx);
            }
            dest0 += VIDEO_FONT_WIDTH * VIDEO_PIXEL_SIZE;
            s++;
        }
        break;
#endif
    case GDF_16BIT_565RGB:
        while (count--) {
            c = *s;
            cdat = video_fontdata + c * VIDEO_FONT_HEIGHT;
            for (rows = VIDEO_FONT_HEIGHT, dest = dest0;
                 rows--;
                 dest += VIDEO_LINE_LEN) {
                u8 bits = *cdat++;

                ((u32 *) dest)[0] = SHORTSWAP32 ((video_font_draw_table16 [bits >> 6] & eorx) ^ bgx);
                ((u32 *) dest)[1] = SHORTSWAP32 ((video_font_draw_table16 [bits >> 4 & 3] & eorx) ^ bgx);
                ((u32 *) dest)[2] = SHORTSWAP32 ((video_font_draw_table16 [bits >> 2 & 3] & eorx) ^ bgx);
                ((u32 *) dest)[3] = SHORTSWAP32 ((video_font_draw_table16 [bits & 3] & eorx) ^ bgx);
            }
            dest0 += VIDEO_FONT_WIDTH * VIDEO_PIXEL_SIZE;
            s++;
        }
        break;
#if 1
    case GDF_32BIT_X888RGB:
        while (count--) {
            c = *s;
            cdat = video_fontdata + c * VIDEO_FONT_HEIGHT;
            for (rows = VIDEO_FONT_HEIGHT, dest = dest0;
                 rows--;
                 dest += VIDEO_LINE_LEN) {
                u8 bits = *cdat++;

                ((u32 *) dest)[0] = SWAP32 ((video_font_draw_table32 [bits >> 4][0] & eorx) ^ bgx);
                ((u32 *) dest)[1] = SWAP32 ((video_font_draw_table32 [bits >> 4][1] & eorx) ^ bgx);
                ((u32 *) dest)[2] = SWAP32 ((video_font_draw_table32 [bits >> 4][2] & eorx) ^ bgx);
                ((u32 *) dest)[3] = SWAP32 ((video_font_draw_table32 [bits >> 4][3] & eorx) ^ bgx);
                ((u32 *) dest)[4] = SWAP32 ((video_font_draw_table32 [bits & 15][0] & eorx) ^ bgx);
                ((u32 *) dest)[5] = SWAP32 ((video_font_draw_table32 [bits & 15][1] & eorx) ^ bgx);
                ((u32 *) dest)[6] = SWAP32 ((video_font_draw_table32 [bits & 15][2] & eorx) ^ bgx);
                ((u32 *) dest)[7] = SWAP32 ((video_font_draw_table32 [bits & 15][3] & eorx) ^ bgx);
            }
            dest0 += VIDEO_FONT_WIDTH * VIDEO_PIXEL_SIZE;
            s++;
        }
        break;

    case GDF_24BIT_888RGB:
        while (count--) {
            c = *s;
            cdat = video_fontdata + c * VIDEO_FONT_HEIGHT;
            for (rows = VIDEO_FONT_HEIGHT, dest = dest0;
                 rows--;
                 dest += VIDEO_LINE_LEN) {
                u8 bits = *cdat++;

                ((u32 *) dest)[0] = (video_font_draw_table24[bits >> 4][0] & eorx) ^ bgx;
                ((u32 *) dest)[1] = (video_font_draw_table24[bits >> 4][1] & eorx) ^ bgx;
                ((u32 *) dest)[2] = (video_font_draw_table24[bits >> 4][2] & eorx) ^ bgx;
                ((u32 *) dest)[3] = (video_font_draw_table24[bits & 15][0] & eorx) ^ bgx;
                ((u32 *) dest)[4] = (video_font_draw_table24[bits & 15][1] & eorx) ^ bgx;
                ((u32 *) dest)[5] = (video_font_draw_table24[bits & 15][2] & eorx) ^ bgx;
            }
            dest0 += VIDEO_FONT_WIDTH * VIDEO_PIXEL_SIZE;
            s++;
        }
        break;
#endif
    }
}

/*****************************************************************************/

static inline void video_drawstring (int xx, int yy, unsigned char *s)
{
    video_drawchars (xx, yy, s, strlen ((char *)s));
}

/*****************************************************************************/

static void video_putchar (int xx, int yy, unsigned char c)
{
    video_drawchars (xx, yy + video_logo_height, &c, 1);
}

/*****************************************************************************/
#if defined(CONFIG_CONSOLE_CURSOR) || defined(CONFIG_VIDEO_SW_CURSOR)
static void video_set_cursor (void)
{
    /* swap drawing colors */
    eorx = fgx;
    fgx = bgx;
    bgx = eorx;
    eorx = fgx ^ bgx;
    /* draw cursor */
    video_putchar (console_col * VIDEO_FONT_WIDTH,
               console_row * VIDEO_FONT_HEIGHT,
               ' ');
    /* restore drawing colors */
    eorx = fgx;
    fgx = bgx;
    bgx = eorx;
    eorx = fgx ^ bgx;
}
#endif

/*****************************************************************************/

static void console_scrollup (void)
{
    /* copy up rows ignoring the first one */
    memcpy (CONSOLE_ROW_FIRST, CONSOLE_ROW_SECOND, CONSOLE_SCROLL_SIZE);

    /* clear the last one */
    memset (CONSOLE_ROW_LAST, 0x0, CONSOLE_ROW_SIZE);
}

/*****************************************************************************/

static void console_back (void)
{
    CURSOR_OFF;
    console_col--;

    if (console_col < 0) {
        console_col = CONSOLE_COLS - 1;
        console_row--;
        if (console_row < 0) {
            console_row = 0;
            console_col = 0;
        }
    }
    video_putchar (console_col * VIDEO_FONT_WIDTH,
               console_row * VIDEO_FONT_HEIGHT,
               ' ');
}

/*****************************************************************************/

static void console_newline (void)
{
    /* Scroll everything up */
    console_scrollup ();

    /* Decrement row number */
    console_row--;
}

static void console_cr (void)
{
    CURSOR_OFF;
    console_col = 0;
}

/*****************************************************************************/

void fbcon_putc (char c)
{
    switch (c) {
    case '\a':       /* bell */
        return;
    case '\r':        /* back to first column */
        console_cr ();
        break;
    case '\t':     /* tab 8 */
        CURSOR_OFF;

        UINT  uiWidth = TAB_WIDTH - (console_col % TAB_WIDTH);
        while (uiWidth--) {
            fbcon_putc(' ');
        }
        return;
    case '\b':     /* backspace */
        console_back ();
        break;

    default:        /* draw the char */
        if (console_col == CONSOLE_COLS || c == '\n') {
            console_col = 0;
            console_row++;
        }
        /* check for newline */
        if (console_row == CONSOLE_ROWS) {
            console_newline ();
        }

        if (c != '\n') {
            video_putchar (console_col * VIDEO_FONT_WIDTH,
                               console_row * VIDEO_FONT_HEIGHT,
                               c);
            console_col++;
        }
    }

    CURSOR_SET;
}


/*****************************************************************************/

static int video_init (void)
{
    video_fb_address = (void *) VIDEO_FB_ADRS;

    fgx = color_table[CONSOLE_FG_COL];
    bgx = color_table[CONSOLE_BG_COL];

    /* Init drawing pats */
    switch (VIDEO_DATA_FORMAT) {
#if 0
    case GDF_15BIT_555RGB:
        fgx = (((CONSOLE_FG_COL >> 3) << 26) |
               ((CONSOLE_FG_COL >> 3) << 21) | ((CONSOLE_FG_COL >> 3) << 16) |
               ((CONSOLE_FG_COL >> 3) << 10) | ((CONSOLE_FG_COL >> 3) << 5) |
               (CONSOLE_FG_COL >> 3));
        bgx = (((CONSOLE_BG_COL >> 3) << 26) |
               ((CONSOLE_BG_COL >> 3) << 21) | ((CONSOLE_BG_COL >> 3) << 16) |
               ((CONSOLE_BG_COL >> 3) << 10) | ((CONSOLE_BG_COL >> 3) << 5) |
               (CONSOLE_BG_COL >> 3));
        break;
#endif
    case GDF_16BIT_565RGB:
        fgx = (RGB888toRGB565(fgx) << 16) | (RGB888toRGB565(fgx));
        bgx = (RGB888toRGB565(bgx) << 16) | (RGB888toRGB565(bgx));
        break;
#if 1
    case GDF_32BIT_X888RGB:
        fgx = fgx;
        bgx = bgx;
        break;
    case GDF_24BIT_888RGB:
        fgx = (CONSOLE_FG_COL << 24) | (CONSOLE_FG_COL << 16) |
            (CONSOLE_FG_COL << 8) | CONSOLE_FG_COL;
        bgx = (CONSOLE_BG_COL << 24) | (CONSOLE_BG_COL << 16) |
            (CONSOLE_BG_COL << 8) | CONSOLE_BG_COL;
        break;
#endif
    }

    eorx = fgx ^ bgx;

    video_console_address = video_fb_address;

    /* Initialize the console */
    console_col = 0;
    console_row = 0;

    return 0;
}

/*****************************************************************************/

int fbcon_get_cols(void)
{
    return CONSOLE_COLS;
}

int fbcon_get_rows(void)
{
    return CONSOLE_ROWS;
}

void  fbcon_set_cursor (int row, int col)
{
    console_col = col;
    console_row = row;
}

void  fbcon_get_cursor (int *row, int *col)
{
    *col = console_col;
    *row = console_row;
}

void  fbcon_set_fgcolor (u8 color)
{
    switch (VIDEO_DATA_FORMAT) {
    case GDF_16BIT_565RGB:
        fgx = (RGB888toRGB565(color_table[color]) << 16) | (RGB888toRGB565(color_table[color]));
        break;
#if 1
    case GDF_32BIT_X888RGB:
    case GDF_24BIT_888RGB:
        fgx = color_table[color];
        break;
#endif
    }

    eorx = fgx ^ bgx;
}


int fbcon_init (void)
{
    LW_GM_VARINFO varInfo;
    int fd, ret;
    size_t screensize = 0;
    void *fb_addr = NULL;

    fd = open("/dev/fb0", O_RDWR);
    if (fd < 0) {
        bspDebugMsg("open /dev/fb0 failed.\r\n");
        return  (-1);
    };

    ret = ioctl(fd, LW_GM_GET_VARINFO, &varInfo);
    if (ret < 0) {
        bspDebugMsg("get /dev/fb0 var info failed.\r\n");
        close(fd);
        return  (-1);
    }

    screensize = varInfo.GMVI_ulXRes * varInfo.GMVI_ulYRes * varInfo.GMVI_ulBytesPerPixel;
    fb_addr = mmap(LW_NULL, screensize, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);
    if (fb_addr == MAP_FAILED) {
        bspDebugMsg("mmap /dev/fb0 failed.\rn");
        close(fd);
        return  (-1);
    }

    pGD = &GD;
    pGD->frameAdrs  = fb_addr;
    pGD->gdfBytesPP = varInfo.GMVI_ulBytesPerPixel;
    pGD->winSizeX   = varInfo.GMVI_ulXRes;
    pGD->winSizeY   = varInfo.GMVI_ulYRes;

    switch (varInfo.GMVI_ulBitsPerPixel) {
    case 16:
        pGD->gdfIndex = GDF_16BIT_565RGB;
        break;
    case 24:
        pGD->gdfIndex = GDF_24BIT_888RGB;
        break;
    case 32:
        pGD->gdfIndex = GDF_32BIT_X888RGB;
        break;
    default:
        pGD->gdfIndex = GDF_32BIT_X888RGB;
    }

    video_init();

    return 0;
}
